#!/usr/bin/python3
import numpy as np
import scipy.linalg as linalg
import math
import random
#参数分别是旋转轴和旋转弧度值
def rotate_mat(axis, radian):
    return linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian))

def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))
    
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    
    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

def eulerAnglesToRotationMatrix(theta) :
    
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
        
        
                    
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
                
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                    
                    
    R = np.dot(R_z, np.dot( R_y, R_x ))

    return R


if __name__ == '__main__' :

    # e = np.random.rand(3) * math.pi * 2 - math.pi
    
    # R = eulerAnglesToRotationMatrix(e)
    # e1 = rotationMatrixToEulerAngles(R)

    # R1 = eulerAnglesToRotationMatrix(e1)
    # print ("\nInput Euler angles :\n{0}".format(e))
    # print ("\nR :\n{0}".format(R))
    # print ("\nOutput Euler angles :\n{0}".format(e1))
    # print ("\nR1 :\n{0}".format(R1))

    # example for rotate_mat
    # axis_x, axis_y, axis_z = [1,0,0], [0,1,0], [0, 0, 1]#分别是x,y和z轴,也可以自定义旋转轴
    # yaw = math.pi/2.0 #pi/4
    # rot_matrix = rotate_mat(axis_z, yaw)#绕Z轴旋转pi/4
    # print(rot_matrix)
    # print("Transpose:")
    # print(rot_matrix.T)
    r = np.array([[0,1,0],[-1,0,0],[0,0,1]])
    e = rotationMatrixToEulerAngles(r)
    print(e)
